/*!
* @file    motor.c
* @version v1.0
* @date    2018-02-05
* @author  苏勇 suyong_yq@126.com
* @brief   实现对小车后驱直流电机的驱动及编码器测速功能
*/

#include "motor.h"

#include "fsl_ctimer.h"
#include "fsl_pint.h"
#include "fsl_mrt.h"
#include "fsl_gpio.h"

/*
* 电机控制插座的四根控制信号：
* P1 - CT1_OUT0 -> 左电机+
* P2 - CT1_OUT1 -> 左电机-
* P3 - CT2_OUT1 -> 右电机-
* P4 - CT2_OUT0 -> 右电机+
*/

/* 左编码器在电路板上标注为EC1
* 脉冲 - CT0_CAP0 -> PIO0_1
* 方向 - DIR1     -> PIO0_2 -> PINT7
*/

/* 右编码器在电路板上标注为EC0
* 脉冲 - CT3_CAP0 -> PIO0_4
* 方向 - DIR0     -> PIO1_15 -> PINT6
*/

/* 指定电机控制用的CTimer模块 */
#define CTIMER_MOTOR_L CTIMER1
#define CTIMER_MOTOR_R CTIMER2

#define CTIMER_ENC_L   CTIMER0
#define CTIMER_ENC_R   CTIMER3

/* 组件级全局变量 */
volatile int32_t gEncL_CurSpeedRAW; /* 左编码器的速度绝对值 */
volatile int32_t gEncR_CurSpeedRAW; /* 右编码器的速度绝对值 */

volatile bool bEncLDirForward; /* 左编码器的速度方向 */
volatile bool bEncRDirForward; /* 右编码器的速度方向 */

volatile bool bEncEnabled = false; /* 两个编码器使用的PINT和MRT均为同一模块的不同通道，公共部分只要初始化一次即可 */

/* 左电机
 * CTIMER1的计数时钟源来自于AHB总线的220MHz，分频得到计数频率为1MHz
 */
void MotorL_InitDriver(void)
{
    ctimer_config_t CTimerConfigStruct;

    CTimerConfigStruct.mode = kCTIMER_TimerMode;
    CTimerConfigStruct.prescale = 220U; /* 从时钟源到计数节拍的分频因子 */
    CTIMER_Init(CTIMER_MOTOR_L, &CTimerConfigStruct);

    /* 配置两个输出控制通道 */
    CTIMER_SetupPwm(CTIMER_MOTOR_L, kCTIMER_Match_0,
        0U,                     /* 初始化输出的占空比 */
        MOTOR_L_SPEED_PWM_FREQ, /* PWM波形频率 */
        220000000UL,            /* 计数原始时钟源频率 */
        false                   /* 是否在每个计数周期产生中断 */);
    CTIMER_SetupPwm(CTIMER_MOTOR_L, kCTIMER_Match_1,
        0U,                     /* 初始化输出的占空比 */
        MOTOR_L_SPEED_PWM_FREQ, /* PWM波形频率 */
        220000000UL,            /* 计数原始时钟源频率 */
        false                   /* 是否在每个计数周期产生中断 */);

    CTIMER_StartTimer(CTIMER_MOTOR_L);
}

/* pwmDuty的符号表示驱动方向，正数为前进，负数为后退
 * 绝对值表示速度
 * pwmDuty的有效范围是[-100,100]
 */
void MotorL_SetSpeed(int32_t pwmDuty)
{
    uint8_t u8Tmp;
    if (pwmDuty > 0)
    {
        CTIMER_UpdatePwmDutycycle(CTIMER_MOTOR_L, kCTIMER_Match_1, 0U);
        u8Tmp = (uint8_t)(pwmDuty % 256);
        if (u8Tmp > MOTOR_L_SPEED_PWM_DUTY_LIMIT)
        {
            u8Tmp = MOTOR_L_SPEED_PWM_DUTY_LIMIT;
        }
        CTIMER_UpdatePwmDutycycle(CTIMER_MOTOR_L, kCTIMER_Match_0, u8Tmp);
    }
    else
    {
    	  CTIMER_UpdatePwmDutycycle(CTIMER_MOTOR_L, kCTIMER_Match_0, 0U);
        u8Tmp = (uint8_t)((-pwmDuty) % 256);
        if (u8Tmp > MOTOR_L_SPEED_PWM_DUTY_LIMIT)
        {
            u8Tmp = MOTOR_L_SPEED_PWM_DUTY_LIMIT;
        }
        CTIMER_UpdatePwmDutycycle(CTIMER_MOTOR_L, kCTIMER_Match_1, u8Tmp);
    }
}

/* 捕获编码器L方向变化的中断事件 */
static void pint7_intr_callback(pint_pin_int_t pintr, uint32_t pmatch_status)
{

    /* 在双边沿触发模式下，不能确定是上升沿还是下降沿触发，此时在读一下触发引脚的电平就能够确定了 */
    /* 向前：PIO0_2读取低电平 */
    if (0U == ((1U << 2) & GPIO_ReadPinsInput(GPIO, 0U)) )
    {
        bEncLDirForward = true;
    }
    else
    {
        bEncLDirForward = false;
    }

    /* 当反转方向时，方向切换点的速度一定是0 */
    /* 通道0 */
    MRT_ChannelUpdateTimerPeriod(MRT0, kMRT_Channel_0,
        CLOCK_GetFreq(kCLOCK_CoreSysClk)/MOTOR_L_SPEED_ENC_INTERVAL_HZ, true);/* 清零计速计数器的值 */
    gEncL_CurSpeedRAW = 0;
    CTIMER_Reset(CTIMER_ENC_L); /* 重新开始计速 */
}

void MotorL_InitEncoder(void)
{
    ctimer_config_t CTimerConfigStruct;    /* CTimer用于捕获编码器脉冲 */
    gpio_pin_config_t GpioPinConfigStruct; /* GPIO及PINT用于捕获方向 */

    /* 初始化MRT基础定时器 */
    if (!bEncEnabled)
    {
        mrt_config_t MrtConfigStruct;  /* MRT用于定期计数，即获取速度信息 */
        MrtConfigStruct.enableMultiTask = false;
        MRT_Init(MRT0, &MrtConfigStruct);
    }

    /* 左编码器使用通道0 */
    MRT_ChannelSetupMode(MRT0, kMRT_Channel_0, kMRT_RepeatMode);
    MRT_ChannelEnableInterrupts(MRT0, kMRT_Channel_0, kMRT_TimerInterruptEnable);

    /* 使用CTimer捕捉脉冲数量 */
    /* 配置CTimer为输入捕捉模式、计数器模式 */
    CTimerConfigStruct.mode = kCTIMER_IncreaseOnFallEdge;
    CTimerConfigStruct.input = kCTIMER_Capture_0; /* 使用CAP0捕获外部脉冲 */
    CTimerConfigStruct.prescale = 0U; /* 从时钟源到计数节拍的分频因子 */
    CTIMER_Init(CTIMER_ENC_L, &CTimerConfigStruct);
    CTIMER_StartTimer(CTIMER_ENC_L);

    /* 使用引脚中断检测方向 */
    /* 初始化PINT */
    if (!bEncEnabled)
    {
        PINT_Init(PINT); /* 芯片内部就一个PINT模块，所以这里也不用定义映射了 */
    }

    PINT_PinInterruptConfig(PINT, kPINT_PinInt7, /* PINT7. */
        kPINT_PinIntEnableBothEdges, /* 上下边沿均可触发 */
        pint7_intr_callback /* 注册触发时执行的函数 */
        );
    /* 启用GPIO模块，辅助判断边沿信息。*/
    //CLOCK_EnableClock(kCLOCK_Gpio0);
    GpioPinConfigStruct.pinDirection = kGPIO_DigitalInput;
    GPIO_PinInit(GPIO, 0U, 2U, &GpioPinConfigStruct); /* PIO0_2. */
    if (0U == ((1U << 2) & GPIO_ReadPinsInput(GPIO, 0U)) )/* 向前：PIO0_2读取低电平 */
    {
        bEncLDirForward = true;
    }
    else
    {
        bEncLDirForward = false;
    }
    if (!bEncEnabled)
    {
        PINT_EnableCallback(PINT); /* Enable callbacks for PINT */
        NVIC_EnableIRQ(MRT0_IRQn); /* 在NVIC中释放 */
        bEncEnabled = true;
    }

    /* 启动定时器定期更新速度变量 */
    //RIT_StartTimer(RIT);
    MRT_ChannelStartTimer(MRT0, kMRT_Channel_0,
        CLOCK_GetFreq(kCLOCK_CoreSysClk)/MOTOR_L_SPEED_ENC_INTERVAL_HZ);
}

/* 返回值的符号表示驱动方向，正数为前进，负数为后退
 * 绝对值表示速度采样值
 */
int32_t MotorL_GetSpeed(void)
{
    int32_t speed;

    if (bEncLDirForward)
    {
        speed = gEncL_CurSpeedRAW;
    }
    else
    {
        speed = -gEncL_CurSpeedRAW; /* 负数表示反方向 */
    }

    return speed;
}

/* 右电机
 * CTIMER2的计数时钟源来自于AHB总线的220MHz，分频得到计数频率为1MHz
 */
void MotorR_InitDriver(void)
{
    ctimer_config_t CTimerConfigStruct;

    CTimerConfigStruct.mode = kCTIMER_TimerMode;
    CTimerConfigStruct.prescale = 220U; /* 从时钟源到计数节拍的分频因子 */
    CTIMER_Init(CTIMER_MOTOR_R, &CTimerConfigStruct);

    /* 配置两个输出控制通道 */
    CTIMER_SetupPwm(CTIMER_MOTOR_R, kCTIMER_Match_0,
        0U,                     /* 初始化输出的占空比 */
        MOTOR_L_SPEED_PWM_FREQ, /* PWM波形频率 */
        220000000UL,            /* 计数原始时钟源频率 */
        false                   /* 是否在每个计数周期产生中断 */);
    CTIMER_SetupPwm(CTIMER_MOTOR_R, kCTIMER_Match_1,
        0U,                     /* 初始化输出的占空比 */
        MOTOR_L_SPEED_PWM_FREQ, /* PWM波形频率 */
        220000000UL,            /* 计数原始时钟源频率 */
        false                   /* 是否在每个计数周期产生中断 */);

    CTIMER_StartTimer(CTIMER_MOTOR_R);
}

/* pwmDuty的符号表示驱动方向，正数为前进，负数为后退
 * 绝对值表示速度
 * pwmDuty的有效范围是[-100,100]
 */
void MotorR_SetSpeed(int32_t pwmDuty)
{
    uint8_t u8Tmp;
    if (pwmDuty > 0)
    {
        CTIMER_UpdatePwmDutycycle(CTIMER_MOTOR_R, kCTIMER_Match_1, 0U);
        u8Tmp = (uint8_t)(pwmDuty % 256);
        if (u8Tmp > MOTOR_R_SPEED_PWM_DUTY_LIMIT)
        {
            u8Tmp = MOTOR_R_SPEED_PWM_DUTY_LIMIT;
        }
        CTIMER_UpdatePwmDutycycle(CTIMER_MOTOR_R, kCTIMER_Match_0, u8Tmp);
    }
    else
    {
    	CTIMER_UpdatePwmDutycycle(CTIMER_MOTOR_R, kCTIMER_Match_0, 0U);
        u8Tmp = (uint8_t)((-pwmDuty) % 256);
        if (u8Tmp > MOTOR_R_SPEED_PWM_DUTY_LIMIT)
        {
            u8Tmp = MOTOR_R_SPEED_PWM_DUTY_LIMIT;
        }
        CTIMER_UpdatePwmDutycycle(CTIMER_MOTOR_R, kCTIMER_Match_1, u8Tmp);
    }
}

/* 捕获编码器R方向变化的中断事件 */
static void pint6_intr_callback(pint_pin_int_t pintr, uint32_t pmatch_status)
{

    /* 在双边沿触发模式下，不能确定是上升沿还是下降沿触发，此时在读一下触发引脚的电平就能够确定了 */
    /* 向前：PIO1_15读取高电平 */
    if (0U == ((1U << 15) & GPIO_ReadPinsInput(GPIO, 1U)) )
    {
        bEncRDirForward = false; /* 右编码器相对于左边是对称的，也就是反的 */
    }
    else
    {
        bEncRDirForward = true;
    }

    /* 当反转方向时，方向切换点的速度一定是0 */
    /* 通道1 */
    MRT_ChannelUpdateTimerPeriod(MRT0, kMRT_Channel_1,
        CLOCK_GetFreq(kCLOCK_CoreSysClk)/MOTOR_L_SPEED_ENC_INTERVAL_HZ, true);/* 清零计速计数器的值 */
    gEncR_CurSpeedRAW = 0;
    CTIMER_Reset(CTIMER_ENC_L); /* 重新开始计速 */
}

void MotorR_InitEncoder(void)
{
    ctimer_config_t CTimerConfigStruct;    /* CTimer用于捕获编码器脉冲 */
    gpio_pin_config_t GpioPinConfigStruct; /* GPIO及PINT用于捕获方向 */

    /* 初始化MRT基础定时器 */
    if (!bEncEnabled)
    {
        mrt_config_t MrtConfigStruct;  /* MRT用于定期计数，即获取速度信息 */
        MrtConfigStruct.enableMultiTask = false;
        MRT_Init(MRT0, &MrtConfigStruct);
    }

    /* 右编码器使用通道1 */
    MRT_ChannelSetupMode(MRT0, kMRT_Channel_1, kMRT_RepeatMode);
    MRT_ChannelEnableInterrupts(MRT0, kMRT_Channel_1, kMRT_TimerInterruptEnable);

    /* 使用CTimer捕捉脉冲数量 */
    /* 配置CTimer为输入捕捉模式、计数器模式 */
    CTimerConfigStruct.mode = kCTIMER_IncreaseOnFallEdge;
    CTimerConfigStruct.input = kCTIMER_Capture_0; /* 使用CAP0捕获外部脉冲 */
    CTimerConfigStruct.prescale = 0U; /* 从时钟源到计数节拍的分频因子 */
    CTIMER_Init(CTIMER_ENC_R, &CTimerConfigStruct);
    CTIMER_StartTimer(CTIMER_ENC_R);

    /* 使用引脚中断检测方向 */
    /* 初始化PINT */
    if (!bEncEnabled)
    {
        PINT_Init(PINT); /* 芯片内部就一个PINT模块，所以这里也不用定义映射了 */
    }

    PINT_PinInterruptConfig(PINT, kPINT_PinInt6, /* PINT6. */
        kPINT_PinIntEnableBothEdges, /* 上下边沿均可触发 */
        pint6_intr_callback /* 注册触发时执行的函数 */
        );
    /* 启用GPIO模块，辅助判断边沿信息。*/
    //CLOCK_EnableClock(kCLOCK_Gpio0);
    GpioPinConfigStruct.pinDirection = kGPIO_DigitalInput;
    GPIO_PinInit(GPIO, 1U, 15U, &GpioPinConfigStruct); /* PIO1_15. */
    if (0U == ((1U << 15) & GPIO_ReadPinsInput(GPIO, 1U)) )/* 向前：PIO1_15读取低电平 */
    {
        bEncRDirForward = true;
    }
    else
    {
        bEncRDirForward = false;
    }
    if (!bEncEnabled)
    {
        PINT_EnableCallback(PINT); /* Enable callbacks for PINT */
        NVIC_EnableIRQ(MRT0_IRQn); /* 在NVIC中释放 */
        bEncEnabled = true;
    }

    /* 启动定时器定期更新速度变量 */
    //RIT_StartTimer(RIT);
    MRT_ChannelStartTimer(MRT0, kMRT_Channel_1,
        CLOCK_GetFreq(kCLOCK_CoreSysClk)/MOTOR_L_SPEED_ENC_INTERVAL_HZ);
}

/* 返回值的符号表示驱动方向，正数为前进，负数为后退
 * 绝对值表示速度采样值
 */
int32_t MotorR_GetSpeed(void)
{
    int32_t speed;

    if (bEncRDirForward)
    {
        speed = gEncR_CurSpeedRAW;
    }
    else
    {
        speed = -gEncR_CurSpeedRAW; /* 负数表示反方向 */
    }

    return speed;
}

/* MRT的中断服务程序入口 */
void MRT0_IRQHandler(void)
{
    /* 通道0，左编码器 */
    if (0U != (kMRT_TimerInterruptFlag & MRT_ChannelGetStatusFlags(MRT0, kMRT_Channel_0)) )
    {
        /* 读取CTimer捕获的脉冲数，加入符号号信息，得到有方向的速度 */
        gEncL_CurSpeedRAW = CTIMER_GetCurrentTimerCounter(CTIMER_ENC_L);
        CTIMER_Reset(CTIMER_ENC_L);
        MRT_ChannelClearStatusFlags(MRT0, kMRT_Channel_0, kMRT_TimerInterruptFlag);
    }

    /* Todo: 通道1，右编码器 */
    if (0U != (kMRT_TimerInterruptFlag & MRT_ChannelGetStatusFlags(MRT0, kMRT_Channel_1)) )
    {
        /* 读取CTimer捕获的脉冲数，加入符号号信息，得到有方向的速度 */
        gEncR_CurSpeedRAW = CTIMER_GetCurrentTimerCounter(CTIMER_ENC_R);
        CTIMER_Reset(CTIMER_ENC_R);
        MRT_ChannelClearStatusFlags(MRT0, kMRT_Channel_1, kMRT_TimerInterruptFlag);
    }
}

/* EOF. */

